import matplotlib.pyplot  as plt
import numpy as np
import os
from pathlib import Path

def plot_error(file_name):
    # 本来一个文件夹下面有三个误差文件，但是我觉得中断时间的不同并不会影响最终 imu 误差的结果，所以画一个，覆盖就覆盖把，老子不管了

    if not os.path.exists(file_name):
        print(file_name + " not exists!")
        return -1
    save_path = os.path.dirname(file_name)
    print(save_path)

    gyro_mode = 0 # 默认上三角
    accel_mode = 0 # 默认上三角
    if "gyroup" in file_name:
        gyro_mode = 0
    elif "gyrodown" in file_name:
        gyro_mode = 1
    if "accelup" in file_name:
        accel_mode = 0
    elif "acceldown" in file_name:
        accel_mode = 1

    try:
        err = np.fromfile(file_name)
        t0 = err[0]
        t7 = err[7]
        t8 = err[8]

        t13 = err[13]
        t14 = err[14]

        t25 = err[25]
        t26 = err[26]
        rows = 7
        if abs(t7 - t0) < 2: # 第二个时间和第一个时间
            err = err.reshape(-1, 7)
        elif abs(t8 - t0) < 2:
            err = err.reshape(-1, 8)
            rows = 8
        elif abs(t13 - t0) < 2:
            err = err.reshape(-1, 13)
            rows = 13
        elif abs(t14 - t0) < 2:
            err = err.reshape(-1, 14)
            rows = 14
        elif abs(t25 - t0) < 2:
            err = err.reshape(-1, 25)
            rows = 25
        elif abs(t26 - t0) < 2:
            err = err.reshape(-1, 26) # 改为26列
            rows = 26
    except Exception:
        print("Please check the imu error file format!")
        return False

    tx = err[:, 0]

    # gyro bias
    gyrbias = plt.figure('imugyrbias')
    plt.plot(tx, err[:, 1], label='X')
    plt.plot(tx, err[:, 2], label='Y')
    plt.plot(tx, err[:, 3], label='Z')
    plt.grid()
    plt.legend()
    plt.xlabel('Time (s)')
    plt.ylabel('Gyroscope Bias (deg/hr)')
    plt.title('Gyroscope Bias')
    plt.tight_layout()
    fig_save = save_path + r"/gyrbias.svg"
    gyrbias.savefig(fig_save, dpi=800)
    # plt.clf()
    # accel bias
    accbias = plt.figure('imuaccbias')
    plt.plot(tx, err[:, 4], label='X')
    plt.plot(tx, err[:, 5], label='Y')
    plt.plot(tx, err[:, 6], label='Z')
    plt.grid()
    plt.legend()
    plt.xlabel('Time (s)')
    plt.ylabel('Accelerometer Bias (mGal)')
    plt.title('Accelerometer Bias')
    plt.tight_layout()
    fig_save = save_path + r"/accbias.svg"
    accbias.savefig(fig_save, dpi=800)
    # plt.clf()
    if rows > 8:
        # gyro scale
        gyrscale = plt.figure('imugyrscale')
        plt.plot(tx, err[:, 7], label='X')
        plt.plot(tx, err[:, 8], label='Y')
        plt.plot(tx, err[:, 9], label='Z')
        plt.grid()
        plt.legend()
        plt.xlabel('Time (s)')
        plt.ylabel('Gyroscope Scale (PPM)')
        plt.title('Gyroscope Scale')
        plt.tight_layout()
        fig_save = save_path + r"/gyrscale.svg"
        gyrscale.savefig(fig_save, dpi=800)
        # plt.clf()
        # accel scale
        accscale = plt.figure('imuaccscale')
        plt.plot(tx, err[:, 10], label='X')
        plt.plot(tx, err[:, 11], label='Y')
        plt.plot(tx, err[:, 12], label='Z')
        plt.grid()
        plt.legend()
        plt.xlabel('Time (s)')
        plt.ylabel('Accelerometer Scale (PPM)')
        plt.title('Accelerometer Scale')
        plt.tight_layout()
        fig_save = save_path + r"/accscale.svg"
        accscale.savefig(fig_save, dpi=800)
        # plt.clf()
    if rows > 20:
        # gyro nonlinearlity
        gyrnon = plt.figure("imugyrononlinearlity")
        if gyro_mode == 0:
            plt.plot(tx, err[:, 13], label='y2x')
            plt.plot(tx, err[:, 14], label='z2x')
            plt.plot(tx, err[:, 15], label='z2y')
        elif gyro_mode == 1:
            plt.plot(tx, err[:, 13], label='x2y')
            plt.plot(tx, err[:, 14], label='x2z')
            plt.plot(tx, err[:, 15], label='y2z')
        plt.plot(tx, err[:, 16], label='fai_x')
        plt.plot(tx, err[:, 17], label='fai_y')
        plt.plot(tx, err[:, 18], label='fai_z')
        plt.grid()
        plt.legend()
        plt.xlabel('Time (s)')
        plt.ylabel('Gyroscope Nonlinearlity (PPM)')
        plt.title('Gyroscope Nonlinearlity')
        plt.tight_layout()
        fig_save = save_path + r"/gyrnon.svg"
        gyrnon.savefig(fig_save, dpi=800)
        # plt.clf()
        # accel nonolinearlity
        accnon = plt.figure("imuaccelnonlinearlity")
        if accel_mode == 0:
            plt.plot(tx, err[:, 19], label='y2x')
            plt.plot(tx, err[:, 20], label='z2x')
            plt.plot(tx, err[:, 21], label='z2y')
        elif accel_mode == 1:
            plt.plot(tx, err[:, 19], label='x2y')
            plt.plot(tx, err[:, 20], label='x2z')
            plt.plot(tx, err[:, 21], label='y2z')
        plt.plot(tx, err[:, 22], label='fai_x')
        plt.plot(tx, err[:, 23], label='fai_y')
        plt.plot(tx, err[:, 24], label='fai_z')
        plt.grid()
        plt.legend()
        plt.xlabel('Time (s)')
        plt.ylabel('Accelerometer Nonlinearlity  (PPM)')
        plt.title('Accelerometer Nonlinearlity Scale')
        plt.tight_layout()
        fig_save = save_path + r"/accnon.svg"
        accnon.savefig(fig_save, dpi=800)
        # plt.clf()
    if rows == 8 or rows == 14 or rows == 26:
        if rows == 8:
            odoscale = err[:, 7]
        elif rows == 14:
            odoscale = err[:, 13]
        elif rows == 26:
            odoscale = err[:, 25]
        plt.figure('odoscale')
        plt.plot(tx, odoscale)
        plt.grid()
        plt.xlabel('Time (s)')
        plt.ylabel('Scale Factor')
        plt.title('Odometer Scale Factor')
        plt.tight_layout()
    plt.show()

if __name__ == "__main__":
    plot_error("/home/wangguan/Desktop/俯仰横滚/data1/result/imuerr/27_acceli_gyroup_acceldown_gny2x_gnz2x_gnz2ygixgiygiz_imu_err.bin")
